home *** CD-ROM | disk | FTP | other *** search
/ The 640 MEG Shareware Studio 2 / The 640 Meg Shareware Studio CD-ROM Volume II (Data Express)(1993).ISO / clang / jcool01.zip / QUATERNI.H < prev    next >
C/C++ Source or Header  |  1992-10-01  |  7KB  |  191 lines
  1. //
  2. // Copyright (C) 1992 General Electric Company.
  3. //
  4. // Permission is granted to any individual or institution to use, copy, modify,
  5. // and distribute this software, provided that this complete copyright and
  6. // permission notice is maintained, intact, in all copies and supporting
  7. // documentation.
  8. //
  9. // General Electric Company,
  10. // provides this software "as is" without express or implied warranty.
  11. //
  12. // Created: VDN 06/23/92 -- design and implementation
  13. // Updated: JAM 09/26/92 -- modernized template syntax, remove macro hacks
  14. // Updated: JAM 09/26/92 -- made envelope use modern templates
  15. // Updated: JAM 09/30/92 -- added operator*=(Quaternion,Quaternion) because
  16. //                          BC++ 3.1 says Envelope needed it
  17. //
  18. // Quaternion is a vector with 1 real and 3 imaginary parts:
  19. //       q = cos(theta/2) + sin(theta/2) (x i + y j + z k)
  20. // Quaternion is represented by a 4-elmt vector, with imaginary parts
  21. // being the first 3 elements, so that they are aligned with x, y, z, and
  22. // the real part, the fourth element.
  23. //
  24. // References:
  25. // 1. Horn, B.K.P. (1987) Closed-form solution of absolute orientation using 
  26. //       unit quaternions. J. Opt. Soc. Am. Vol 4, No 4, April.
  27. // 2. Horn, B.K.P. (1987) Robot Vision. MIT Press. pp. 437-551.
  28.  
  29. #ifndef QUATERNIONH
  30.  
  31. #ifndef MATRIXH
  32. #include <cool/Matrix.h>
  33. #endif
  34.  
  35. #ifndef M_VECTORH
  36. #include <cool/M_Vector.h>
  37. #endif
  38.  
  39. //## hack to workaround BC++ 3.1 Envelope bug
  40. #undef CoolEnvelope_H
  41. #define CoolEnvelope CoolEnvelope_Quaternion
  42.  
  43. template<class CoolLetter> class CoolEnvelope;
  44.  
  45. class CoolQuaternion : public CoolM_Vector<float> {
  46.  public:
  47.   CoolQuaternion (float x = 0, float y = 0, float z = 0, // default is null quat.
  48.           float r = 1.0); 
  49.   CoolQuaternion (const CoolM_Vector<float>& axis, // from axis&angle
  50.           float angle);            
  51.   CoolQuaternion (const CoolMatrix<float>& transform); // from 2-4 square row-major
  52.   inline CoolQuaternion (const CoolM_Vector<float>& vec); // from 2-4D vector
  53.   inline CoolQuaternion (const CoolQuaternion& q);      // copy constructor
  54.   inline ~CoolQuaternion();                  // free internal array
  55.   
  56.   inline float& x ();                // imaginary component
  57.   inline float& y ();                // parallel to axis of rotation
  58.   inline float& z ();
  59.   inline float& r ();                // real component
  60.   inline float real () const;                
  61.   inline CoolEnvelope_M_Vector/*##*/< CoolM_Vector<float> > imaginary () const; // imaginary vector part
  62.   
  63.   CoolEnvelope_M_Vector/*##*/< CoolM_Vector<float> > axis () const;        // Axis of rotation
  64.   float angle () const;                // Angle of rotation
  65.   
  66.   inline CoolQuaternion& operator= (const CoolQuaternion& rhs);    // q1 = q2
  67.   inline CoolQuaternion& operator= (CoolEnvelope<CoolQuaternion>& env);    // q1 = env
  68.   
  69.   Boolean operator== (const CoolQuaternion& rhs) const; // cmp with fuzz = 1.0e-6
  70.   inline Boolean operator!= (const CoolQuaternion& rhs) const; // instead of 1.0e-8
  71.   
  72.   inline operator CoolM_Vector<float>& ();    // cast to 4-elmt vector
  73.   CoolEnvelope_Matrix/*##*/< CoolMatrix<float> > rotation_transform (int dim = 4) const; // to 2-4 rot matrix
  74.   
  75.   CoolEnvelope<CoolQuaternion> conjugate () const;        // same real, opposite img part
  76.   CoolEnvelope<CoolQuaternion> inverse () const;        // inverse for nonzero quat
  77.   
  78.   friend CoolEnvelope<CoolQuaternion> operator* (const CoolQuaternion& q1, // q = q1 * q2 * q3
  79.                     const CoolQuaternion& q2);
  80.   void rotate (CoolM_Vector<float>& v) const;    // rotate 3D v, store result in v.
  81.   
  82.   /*inline##*/ friend ostream& operator<< (ostream& os, const CoolQuaternion& q);
  83.   /*inline##*/ friend ostream& operator<< (ostream& os, const CoolQuaternion* q);
  84. };
  85.  
  86. //## BC++ 3.1 bug
  87. void hack(CoolQuaternion);
  88.  
  89. #include <cool/Envelope.h>    //## BC++ 3.1 bug prevents from moving to top
  90.  
  91.  
  92. // Quaternion -- Construct Quaternion from 2-, 3- or 4-elmt vector.
  93. //             If nD-vector, construct imaginary Quaternion.
  94. // Input:      2D, 3D location vector, or 4D Quaternion vector.
  95.  
  96. inline CoolQuaternion::CoolQuaternion (const CoolM_Vector<float>& vec)
  97. : CoolM_Vector<float>(vec) {            // 1-1 layout between vector&quat
  98. }
  99.  
  100. // Quaternion -- Copy constructor
  101. // Input:      Quaternion
  102.  
  103. inline CoolQuaternion::CoolQuaternion (const CoolQuaternion& q) 
  104.   : CoolM_Vector<float>(q) {            // 1-1 layout between vector&quat
  105. }
  106.  
  107. // ~Quaternion -- Nothing, since Quaternion has same data as Vector.
  108.  
  109. inline CoolQuaternion::~CoolQuaternion () {}    // Vector will free data
  110.  
  111. // x -- Access first imaginary component, along x axis.
  112.  
  113. inline float& CoolQuaternion::x () {
  114.   return this->data[0];
  115. }
  116.  
  117. // y -- Access second imaginary component, along y axis.
  118.  
  119. inline float& CoolQuaternion::y () {
  120.   return this->data[1];
  121. }
  122.  
  123. // z -- Access third imaginary component, along z axis.
  124.  
  125. inline float& CoolQuaternion::z () {
  126.   return this->data[2];
  127. }
  128.  
  129. // r -- Access real component
  130.  
  131. inline float& CoolQuaternion::r () {
  132.   return this->data[3];
  133. }
  134.  
  135. // real -- Get real part
  136.  
  137. inline float CoolQuaternion::real () const {
  138.   return this->data[3];
  139. }
  140.  
  141. // imaginary -- Get imaginary part
  142.  
  143. inline CoolEnvelope_M_Vector/*##*/< CoolM_Vector<float> > CoolQuaternion::imaginary () const {
  144.   return this->extract(3,0);
  145. }
  146.  
  147.   // operator=  -- Assignment q1 = q2;
  148. // Input:    Quaternion on rhs
  149. // Output:   Quaternion on lhs, with contents of Quaternion on rhs copied over.
  150.  
  151. inline CoolQuaternion& CoolQuaternion::operator= (const CoolQuaternion& rhs) {
  152.   CoolM_Vector<float>::operator=(rhs);        // same as copy vector part
  153.   return *this;
  154. }
  155.  
  156. // operator=  -- Assignment from an envelope back to real Quaternion
  157. //            Swap the contents over, rhs envelope will be deleted by compiler.
  158.  
  159. inline CoolQuaternion& CoolQuaternion::operator= (CoolEnvelope<CoolQuaternion>& env) {
  160.   env.shallow_swap((CoolEnvelope<CoolQuaternion>*)this, &env); // same physical layout
  161.   return *this;  
  162. }
  163.  
  164. // operator!= -- Components of Quaternion are compared with fuzz = 1.0e-6
  165.  
  166. inline Boolean CoolQuaternion::operator!= (const CoolQuaternion& rhs) const {
  167.   return (!operator==(rhs));
  168. }
  169.  
  170. // operator M_Vector  --  Automatic conversion to a 4-elmt vector.
  171. // Input:    *this, a Quaternion.
  172. // Output:   a reference to the 4-elmt vector, through a type-cast.
  173.  
  174. inline CoolQuaternion::operator CoolM_Vector<float>& () {
  175.   return *((CoolM_Vector<float>*) this);    // same physical space
  176. }
  177.  
  178. // operator<<  -- Print the components of Quaternion.
  179.  
  180.   inline ostream& operator<< (ostream& os, const CoolQuaternion& q) {
  181.   return os << *((CoolM_Vector<float>*) &q);
  182. }
  183.  
  184. inline ostream& operator<< (ostream& os, const CoolQuaternion* q) {
  185.   return os << *((CoolM_Vector<float>*) q);
  186. }
  187.  
  188. #undef CoolEnvelope
  189.  
  190. #endif                        // QuaternionH
  191.